#ifndef HERMIT4_SSE_TH_H
#define HERMIT4_SSE_TH_H

#include <math.h>
#include <xmmintrin.h>
#include <iostream>
#include <string.h>

/* Thanks to Bathil Paledorius !!! */


// my includes
#include "utils.02.h"
#include "fjf_nn2_sse_cor_th_n.01.h"
#include "Energy.h"

namespace dj01 {

	
class Hermit4_SSE_TH : public FJF_NN2_SSE_COR_TH_n {
	
	public:
	
	double *t;
	double *dt;
	uint *pid;
	uint *pid_act;
	
	double *x0p, *x1p, *x2p;
	double *v0p, *v1p, *v2p;
	
		
	double period;
	double dt_min;
	double t_min;
	
	lluint n_bts, n_bts_tot;
	lluint s_n, s_n_tot;
	lluint s_nnij, s_nnij_tot;
	lluint s_nn2ij, s_nn2ij_tot;
	
	Timer tr;
	
	Energy E;
	
	char *pf0,*pf1,*pf2, *pj0,*pj1,*pj2, *ppot;
	char *px0p,*px1p,*px2p, *pv0p,*pv1p,*pv2p;
	
	
	virtual void integrate(uint N, double *m, double*x0,double*x1,double*x2, double*v0,double*v1,double*v2, double eps2, uint n_core, double period, double dt_min) {
		
		tr.set();
		
		uint align=16;
		
		uint _N=N+1;
		
		// COMMON DATA
		if (this->N!=N) {
			
			if (this->n_core) {
				delete [] t;
				delete [] dt;
				delete [] pid;
				delete [] pid_act;
				
				delete [] pf0;
				delete [] pf1;
				delete [] pf2;
		
				delete [] pj0;
				delete [] pj1;
				delete [] pj2;
		
				delete [] ppot;
				
				
				delete [] px0p;
				delete [] px1p;
				delete [] px2p;
		
				delete [] pv0p;
				delete [] pv1p;
				delete [] pv2p;
				
				
				
			}
			
			t = new double[N];
			dt = new double[N];
			pid = new uint[N];
			pid_act = new uint[N];
			

			f0 = (double*)new_align(_N, 8, align, pf0);
			f1 = (double*)new_align(_N, 8, align, pf1);
			f2 = (double*)new_align(_N, 8, align, pf2);
	
			j0 = (double*)new_align(_N, 8, align, pj0);
			j1 = (double*)new_align(_N, 8, align, pj1);
			j2 = (double*)new_align(_N, 8, align, pj2);
		
			pot = (double*)new_align(_N, 8, align, ppot);	

			x0p = (double*)new_align(_N, 8, align, px0p);
			x1p = (double*)new_align(_N, 8, align, px1p);
			x2p = (double*)new_align(_N, 8, align, px2p);
	
			v0p = (double*)new_align(_N, 8, align, pv0p);
			v1p = (double*)new_align(_N, 8, align, pv1p);
			v2p = (double*)new_align(_N, 8, align, pv2p);
			
			
		}
		
		memset( t,0,N*sizeof(double));
		memset(dt,0,N*sizeof(double));
		
		// THREADS DATA
		if ( this->n_core && (this->N!=N || this->n_core!=n_core) )   // recreate x0s.., th[i].f0.. if N or n_core changed
			
			for (uint i=0; i<this->n_core; i++) 
			{
				delete [] th[i].f0p;
				delete [] th[i].f1p;
				delete [] th[i].f2p;
					 
				delete [] th[i].j0p;
				delete [] th[i].j1p;
				delete [] th[i].j2p; 
					
				delete [] th[i].potp; // delete old array
			
			}

		if (this->n_core!=n_core)	{
			
			if (this->n_core) delete [] th;
			
			th = new TH[n_core];
			
			for (uint i=0; i<n_core; i++) {
				th[i].id = i;
				th[i].ff = this;
			}
			//printf("Threads Vars are created.\n");
			pthread_barrier_init(&barrier, NULL, n_core);
		}
		
		if (this->N!=N || this->n_core!=n_core)
			for (uint i=0; i<n_core; i++) {
			
				th[i].f0 = (double*)new_align(_N, 8, align, th[i].f0p); //*10 -performance reason
				th[i].f1 = (double*)new_align(_N, 8, align, th[i].f1p);
				th[i].f2 = (double*)new_align(_N, 8, align, th[i].f2p);
				
				th[i].j0 = (double*)new_align(_N, 8, align, th[i].j0p); //*10 -performance reason
				th[i].j1 = (double*)new_align(_N, 8, align, th[i].j1p);
				th[i].j2 = (double*)new_align(_N, 8, align, th[i].j2p);
				
				th[i].pot = (double*)new_align(_N, 8, align, th[i].potp);
				
				
				
				//printf("%u: ThArrs are created\n", i);
			}
		
		//printf("Integration Initiliztation:\n");
		//printf("N=%u\n", N);
		//printf("eps2=%g\n", eps2);


		this->dt_min = dt_min;
		this->period = period;

		this->n_core = n_core;
 		this->N = N;

		this->m = m;
		
		this->x0 = x0;
		this->x1 = x1;
		this->x2 = x2;
		
		this->v0 = v0;
		this->v1 = v1;
		this->v2 = v2;
		
		this->eps2 = eps2;

		
		n_bts = s_n = s_nnij = s_nn2ij = 0;

		
		for (uint i=0; i<n_core; i++)
			pthread_create(&th[i].pth, NULL, &thread_luncher, (void*)&th[i]);
	
		for (uint i=0; i<n_core; i++)
			pthread_join(th[i].pth, NULL);
		/**/
		
		// calc some stuff....
		
		tr.get();
	}
	
	
	struct TH : FJF_NN2_SSE_COR_TH_n::TH{
		uint *id_act;
		uint n;
		double t_min;
		
		Timer tr;
		
	}*th;
	
	
	void predict_xv(	uint i1,uint i2, 
					double*x0,double*x1,double*x2, double*v0,double*v1,double*v2, 
					double*f0,double*f1,double*f2, double*j0,double*j1,double*j2, 
					double*t, double t_min,
					double*x0p,double*x1p,double*x2p, double*v0p,double*v1p,double*v2p){
		
		
		for (uint i=i1; i<=i2; i++) {
			
			double dt1 = t_min-t[i];                        
			double dt2 = SQR(dt1);
			double dt2over2 = dt2*0.5000000000000000;
			double dt3over6 = dt1*dt2*0.1666666666666666;
			
			double a00=f0[i]/m[i];
			double a01=f1[i]/m[i];
			double a02=f2[i]/m[i];
			
			double a10=j0[i]/m[i];
			double a11=j1[i]/m[i];
			double a12=j2[i]/m[i];
			
			x0p[i] = x0[i] + v0[i]*dt1 + a00*dt2over2 + a10*dt3over6;
			x1p[i] = x1[i] + v1[i]*dt1 + a01*dt2over2 + a11*dt3over6;
			x2p[i] = x2[i] + v2[i]*dt1 + a02*dt2over2 + a12*dt3over6;
			
			v0p[i] = v0[i] + a00*dt1 + a10*dt2over2;
			v1p[i] = v1[i] + a01*dt1 + a11*dt2over2;
			v2p[i] = v2[i] + a02*dt1 + a12*dt2over2;
			
			
		}
	}
	
	template <typename V>
	inline void swap(V *p1, V *p2) {
		V tmp = *p1;
		*p1 = *p2;
		*p2 = tmp;
	}
	
	
	
	virtual void thread(uint id) {
		
		TH &thi = th[id];
		
		
		uint i1, i2; // sum coordinates
		double eta=0.02; //
		double eta_s=eta/4; //
		
		double dt_min = 1.0*pow(2.0, -23); // TEMP
		double dt_max = 1.0*pow(2.0, -3);  // TEMP
		
		//double 
		t_min = 0;
		
				
		// define the initial f[] and j[] (a0, a1)
		thi.tr.set();
		lluint nij = (lluint)N*N-((lluint)N*(N+1))/2;
		set_ncoreopt_nijmaxmin(nij, n_core, nij_max, nij_min);
		set_work_cor(id, n_core, N, N, nij_max, thi.i1_,thi.j1_,thi.i2_,thi.j2_);
		correct_work_cor(N, thi.i1_,thi.j1_,thi.i2_,thi.j2_);
		forcefinder.ff(N, N,  thi.i1_,thi.j1_,thi.i2_,thi.j2_,  m, x0,x1,x2,  v0,v1,v2,  thi.f0,thi.f1,thi.f2,  thi.j0,thi.j1,thi.j2, thi.pot, eps2);
		thi.tr.get();
	
		pthread_barrier_wait(&barrier); // MANDATORY
		
		/*
		if (id==0)
		for(uint i=0; i<N; i+=1000)
			printf("%4u: % .15e % .15e % .15e\n", i, f0[i]/m[i], f1[i]/m[i], f2[i]/m[i]);

		return;
		*/
		
		
		
		// define in parallel:
		// 1. the initial dt[i]
		// 2, the local number of particles with smallest t_min=t[i]+dt[i] :: thi.n
		// 3. the local lists of particles with smallest t_min=t[i]+dt[i] :: thi.id_act[]
		 
		if (id==0) {
			
			for (uint i=0; i<N; i++) {
			
				// reduce values
				f0[i]=f1[i]=f2[i]=j0[i]=j1[i]=j2[i]=pot[i]=0;	
				for (uint j=0; j<n_core; j++) {	
				
					f0[i]+=th[j].f0[i]; 
					f1[i]+=th[j].f1[i];	
					f2[i]+=th[j].f2[i];
				
					j0[i]+=th[j].j0[i]; 
					j1[i]+=th[j].j1[i];	
					j2[i]+=th[j].j2[i];
				
					pot[i]+=th[j].pot[i]; 
				
				}
			
				double f2M = SQR(f0[i])+SQR(f1[i])+SQR(f2[i]);
				double j2M = SQR(j0[i])+SQR(j1[i])+SQR(j2[i]);
				double dti = j2M ?  eta_s*sqrt(f2M/j2M) : eta_s;
			
				dti = pow(2.0, int(log(dti)/log(2.0)-1));
				if (dti < dt_min) dti = dt_min;
				if (dti > dt_max) dti = dt_max;
			
				if (dti > period) dti = period;
				dt[i] = dti;

				pid[i]=i;
			}

			E.calc(N,m,v0,v1,v2,pot);
			
			s_n+=N;
			s_nn2ij+=nij;
		}
	

		
		pthread_barrier_wait(&barrier);	
		
		do {
		
		
		if (id==0) {
			
			// find t_min and n
			n=0;
			t_min=period;
			
			for (uint i=0; i<N; i++) {
			
				// 1. minimums of t[i]+dt[i] :: t_min
				double ti_min = t[i]+dt[i];
				if (t_min > ti_min){
					t_min = ti_min;
					n=0;
				}
			
				// 2. number of perticles with local minimim of t[i]+dt[i] :: n
				if (t_min==ti_min) { 
					pid_act[n]=i;
					n++;
				}
			}
			
			
			// reorder m, x, v arrays; to shift active particle in the beginig of arrays	
			for (uint i=0; i<n; i++) {
						
				uint id = pid_act[i];
						
				swap(&m[id], &m[i]);
							
				swap(&x0[id], &x0[i]);
				swap(&x1[id], &x1[i]);
				swap(&x2[id], &x2[i]);
							
				swap(&v0[id], &v0[i]);
				swap(&v1[id], &v1[i]);
				swap(&v2[id], &v2[i]);
						
				swap(&f0[id], &f0[i]);
				swap(&f1[id], &f1[i]);
				swap(&f2[id], &f2[i]);
						
				swap(&j0[id], &j0[i]);
				swap(&j1[id], &j1[i]);
				swap(&j2[id], &j2[i]);
						
				swap( &t[id], &t[i]);
				swap(&dt[id],&dt[i]);
						
				swap(&pid[id],&pid[i]);
				
			}
			
			
			predict_xv(0, N-1, x0,x1,x2, v0,v1,v2, f0,f1,f2, j0,j1,j2, t, t_min, x0p,x1p,x2p, v0p,v1p,v2p);	
		}
		
		
		
		pthread_barrier_wait(&barrier);  // MANDATORY
		
		//t_min
		
		
		// calc a0, a1 for active prticles
		thi.tr.set();
		lluint nij = (lluint)N*n-((lluint)n*(n+1))/2;
		set_ncoreopt_nijmaxmin(nij, n_core, nij_max, nij_min);
		set_work_cor(id, n_core, n, N, nij_max, thi.i1_,thi.j1_,thi.i2_,thi.j2_);
		correct_work_cor(N, thi.i1_,thi.j1_,thi.i2_,thi.j2_);
		forcefinder.ff(n, N, thi.i1_,thi.j1_,thi.i2_,thi.j2_, m, x0p,x1p,x2p, v0p,v1p,v2p, thi.f0,thi.f1,thi.f2, thi.j0,thi.j1,thi.j2, thi.pot, eps2);
		thi.tr.get();
		
		pthread_barrier_wait(&barrier);  // MANDATORY
		
		
		// 1. Correct predicted x, v
		// 2. Redefine timesteps for active particles according to full way 
		
		if (id==0) {
			
		for (uint i=0; i<n; i++) {
			
			double f0pi(0),f1pi(0),f2pi(0), j0pi(0),j1pi(0),j2pi(0);
			
			for (uint j=0; j<n_core; j++) {
			
				f0pi+=th[j].f0[i];
				f1pi+=th[j].f1[i];
				f2pi+=th[j].f2[i];
			
				j0pi+=th[j].j0[i];
				j1pi+=th[j].j1[i];
				j2pi+=th[j].j2[i];
			}


			double dti=dt[i];
		
			
			double dti2=dti*dti;
			double dti3=dti2*dti;
			double dti4=dti2*dti2;
			double dti5=dti4*dti;
			
			double minus6_over_dti2 = -6/dti2;
			double minus2_over_dti = -2/dti;
			double plus12_over_dti3 = 12/dti3;
			double plus6_over_dti2 = 6/dti2;
		
			double dti3_over_6 = dti3/6;
			double dti4_over_24 = dti4/24;
			double dti5_over_120 = dti5/120;

			// correction
			double a20i = (minus6_over_dti2*(f0[i]-f0pi) + minus2_over_dti*(2*j0[i]+j0pi))/m[i];
			double a21i = (minus6_over_dti2*(f1[i]-f1pi) + minus2_over_dti*(2*j1[i]+j1pi))/m[i];
			double a22i = (minus6_over_dti2*(f2[i]-f2pi) + minus2_over_dti*(2*j2[i]+j2pi))/m[i];
			
			double a30i = (plus12_over_dti3*(f0[i]-f0pi) + plus6_over_dti2*(j0[i]+j0pi))/m[i];
			double a31i = (plus12_over_dti3*(f1[i]-f1pi) + plus6_over_dti2*(j1[i]+j1pi))/m[i];
			double a32i = (plus12_over_dti3*(f2[i]-f2pi) + plus6_over_dti2*(j2[i]+j2pi))/m[i];
			
			x0[i]= x0p[i] + a20i*dti4_over_24 + a30i*dti5_over_120;
			x1[i]= x1p[i] + a21i*dti4_over_24 + a31i*dti5_over_120;
			x2[i]= x2p[i] + a22i*dti4_over_24 + a32i*dti5_over_120;
						
			v0[i]= v0p[i] + a20i*dti3_over_6 + a30i*dti4_over_24;
			v1[i]= v1p[i] + a21i*dti3_over_6 + a31i*dti4_over_24;
			v2[i]= v2p[i] + a22i*dti3_over_6 + a32i*dti4_over_24;
			

			double a0piM = sqrt( SQR(f0pi/m[i]) + SQR(f1pi/m[i]) + SQR(f2pi/m[i]) );
			double a1piM = sqrt( SQR(j0pi/m[i]) + SQR(j1pi/m[i]) + SQR(j2pi/m[i]) );
			
			double a20pi = a20i + dti*a30i;
			double a21pi = a21i + dti*a31i;
			double a22pi = a22i + dti*a32i;
			
			double a2piM = sqrt( SQR(a20pi) + SQR(a21pi) + SQR(a22pi) );
			double a3piM = sqrt( SQR(a30i) + SQR(a31i) + SQR(a32i) );
			
			double dti_new = sqrt( eta * (a0piM*a2piM + SQR(a1piM)) / (a1piM*a3piM + SQR(a2piM)) );

		//	printf("dt[%u]_new[%u]: %.16g\n", i, id, dti_new);
			
			// mine...

			//dti_new =       pow(2.0, int(log(dti_new)/log(2.0)-1)); =>
			//if (dti_new < dt_min) dti_new = dt_min;
			//if (dti_new > 2.0*dt[i]) dti_new = 2.0*dt[i];
			//if (dti_new > period-t[i]) dti_new = period-t[i];
			
			// Berzick
			double dti_tmp = t_min-t[i];
			
		   if(dti_new < dt_min) dti_tmp = dt_min;
			
			if( (dti_new < dti_tmp) && (dti_new > dt_min) ) {
				int power = log(dti_new)/log(2.0) - 1;
				dti_tmp = pow(2.0, (double)power);
			}

			if( fmod(t_min, 2*dti_tmp)	==	.0				&& 
								 2*dti_tmp  <= dt_max		&&
									dti_new	>	2*dti_tmp 	) dti_tmp*= 2;
				
			
			
			dt[i] = dti_tmp;
			t[i]=t_min; 
			
			f0[i]= f0pi;
			f1[i]= f1pi;
			f2[i]= f2pi;
			
			j0[i]= j0pi;
			j1[i]= j1pi;
			j2[i]= j2pi;
			
			
		}
		
		
		n_bts++;
		s_n+=n;
		s_nnij+=(luint)N*n-(luint)n*n;
		s_nn2ij+=((lluint)n*(n-1))/2;;
		
		printf("%u: %4u\t%.15f\t%u\n", id, n_bts, t_min, n);
		}
		
		//pthread_barrier_wait(&barrier);  // MANDATORY
		
		} while (t_min<period);
		//} while (stp<2);
		
		printf("Timings[%u]: %.2f %.2f\n", id, thi.tr.sin_tot, thi.tr.sout_tot);
		
		pthread_barrier_wait(&barrier);  // MANDATORY
		
		if (id==0) {
			// reduce pot
			for (uint i=0; i<N; i++) {
				pot[i]=0;
				for (uint j=0; j<n_core; j++)
				pot[i]+=th[j].pot[i];
			}	
			E.calc(N,m,v0,v1,v2,pot);
		}
		
		
		
		
	}
};

}
#endif
